#include "chassis_calculate.h"

#define PI 3.14159265


wheel_group_dataTypeDef chassis_wheel_group[4] = {};
pid_controller chassis_rudder_angle_pid[4] = {};

float chassis_driving_factor = 50;

void chassis_calculate_init()
{
	for (int i = 0; i < 2; i++)
	{
		pid_controller_create(chassis_rudder_angle_pid + i, 2, 0, 0, 1000, PI);
	}
}

float chassis_calculate_angle_normal(float angle)
{
	if(angle > PI)
	{
		while(angle > PI)
		{
			angle -= (2 * PI);
		}
	}
	else if (angle < - PI)
	{
		while (angle < - PI)
		{
			angle += (2 * PI);
		}
	}
	return angle;
}
	
void chassis_calculate_wheel_group(float vx, float vy, float vz, float power_factor)
{
	if(fabs(vx) + fabs(vy) + fabs(vz) < 0.3)
	{
		for (int i = 0; i < 2; i++)
		{
			wheel_group_rudder_speed_target[i] = 0;
			wheel_group_driving_speed_target[i] = 0;
		}
		return;
	}

	chassis_wheel_group[0].angle = atan2(vy + vz, vx);
	chassis_wheel_group[1].angle = atan2(vy - vz, vx);
	chassis_wheel_group[2].angle = atan2(vy, vx - vz);
	chassis_wheel_group[3].angle = atan2(vy, vx + vz);
	
	chassis_wheel_group[0].driving_speed = sqrt(pow(vy + vz, 2) + pow(vx, 2));
	chassis_wheel_group[1].driving_speed = sqrt(pow(vy - vz, 2) + pow(vx, 2));
	chassis_wheel_group[2].driving_speed = sqrt(pow(vy, 2) + pow(vx - vz, 2));
	chassis_wheel_group[3].driving_speed = sqrt(pow(vy, 2) + pow(vx + vz, 2));
	
	
	
	float speed_sum = 0;
	
	for (int i = 0; i < 2; i++)
	{
		speed_sum += chassis_wheel_group[i].driving_speed;
	}
	
	for (int i = 0; i < 2; i++)
	{
		float error_angle = chassis_wheel_group[i].angle - wheel_group_data[i].angle;
		while (error_angle > PI)
		{
			error_angle = error_angle - 2 * PI;
		}
		while (error_angle < - PI)
		{
			error_angle = error_angle + 2 * PI;
		}
		if (error_angle > 0.5 * PI)
		{
			error_angle = error_angle - PI;
			chassis_wheel_group[i].driving_speed = - chassis_wheel_group[i].driving_speed;
		}
		else if (error_angle < - 0.5 * PI)
		{
			error_angle = error_angle + PI;
			chassis_wheel_group[i].driving_speed = - chassis_wheel_group[i].driving_speed;
		}
		pid_error_input(chassis_rudder_angle_pid + i, - error_angle);
		chassis_wheel_group[i].rudder_speed = chassis_rudder_angle_pid[i].output;
		speed_sum += fabs(chassis_wheel_group[i].rudder_speed);
	}
	
	for (int i = 0; i < 2; i++)
	{
		wheel_group_rudder_speed_target[i] = chassis_wheel_group[i].rudder_speed * power_factor / speed_sum;
		wheel_group_driving_speed_target[i] = chassis_wheel_group[i].driving_speed * chassis_driving_factor * power_factor / speed_sum;
	}
}
